#include "particle.h"
#include "fitness_functions.h"
#include "pso.h"
#include <limits>


Particle::Particle(Pso *p_pso)
    : m_pso(p_pso)
{
    // Init postions randomly.
    m_current_position = new Position(m_pso->getParams()->dimensions,
        m_pso->getParams()->boundary_radius);
    m_best_position = new Position(*m_current_position);

    // Init velocity of particle as 0.
    m_velocity = new Velocity(m_pso->getParams()->dimensions, 
        m_pso->getParams()->boundary_radius);

    m_fitness = std::numeric_limits<double>::max();
    m_best_fitness = std::numeric_limits<double>::max();

    m_calculated_fitness = false;
}

Particle::~Particle()
{
    delete m_current_position;
    delete m_best_position;
    delete m_velocity;
}

void Particle::calculateFitness()
{    
    fitness::fitness_function function;

    function = fitness::getFitnessFunction(
        m_pso->getParams()->fitness_function);
    m_fitness = function(m_current_position);
    
    // Shall we update our personal best position
    // ... and fitness? We are minimizing.
    if(m_fitness < m_best_fitness) {
        // We have a new winner.
        m_best_fitness = m_fitness;

        // Copy the position.
        delete m_best_position;
        m_best_position = new Position(*m_current_position);
    }
    
    m_calculated_fitness = true;
}

double Particle::getFitness()
{
    if(m_calculated_fitness == false) {
        // We have not calculated this particle's fitness yet.
        calculateFitness();
    }

    return m_fitness;
}

const Velocity *Particle::getVelocity()
{
    return m_velocity;
}

const Position *Particle::getPosition()
{
    return m_current_position;
}

const Position *Particle::getBestPosition()
{
    return m_best_position;
}

void Particle::setPosition(const Position &p_position)
{
    (*m_current_position) = p_position;

    // We have to calculate the fitness again because our position
    // ... has changed.
    m_calculated_fitness = false;
}

void Particle::setVelocity(const Velocity &p_velocity)
{
    (*m_velocity) = p_velocity;
}

Particle &Particle::operator=(const Particle &p_particle)
{
    Position *current_position = NULL;
    Position *best_position = NULL;
    Velocity *velocity = NULL;

    // No self assignment for you!
    if(this == &p_particle) {
        return *this;
    }

    // Create the new instances of the things we want.
    current_position = new Position(*p_particle.m_current_position);
    best_position = new Position(*p_particle.m_best_position);
    velocity = new Velocity(*p_particle.m_velocity);

    // Erase the old instances.
    delete m_current_position;
    delete m_best_position;
    delete m_velocity;

    // Assign the new ones.
    m_current_position = current_position;
    m_best_position = best_position;
    m_velocity = velocity;

    // Copy simple values over.
    m_pso = p_particle.m_pso;
    m_fitness = p_particle.m_fitness;
    m_best_fitness = p_particle.m_best_fitness;
    m_calculated_fitness = p_particle.m_calculated_fitness;

    return *this;
    
}
